#include "trdpthread.h"
#include <QDebug>

TRDPThread::TRDPThread()
    : err(TRDP_NO_ERR),m_state(stop)
{
    m_state= stop;
    if(GlobalConfig::getCurrentBoardId()==1)
        ownIP = GlobalConfig::cabNumber==1? 0x0A010111:0x0A010611;
    else if(GlobalConfig::getCurrentBoardId()==2)
        ownIP = GlobalConfig::cabNumber==1? 0x0A000111:0x0A000611;
    qDebug()<<"this HMI ip is :"<<hex<<ownIP<<dec;
}

TRDPThread::~TRDPThread()
{

}

void TRDPThread::add_pd_sinkPort(TRDPDataPortInfo &port)
{
    /* add ports to config */
    for(int i=0;i<gRecvPortCnt;++i)
    {
        if(gRecvPacket[i].comID == port.comid &&
           gRecvPacket[i].src == vos_dottedIP(port.srcipinfo.srcip.toStdString().c_str()) &&
           gRecvPacket[i].dst == vos_dottedIP(port.destip.toStdString().c_str()))
            return;
    }
    gRecvPacket[gRecvPortCnt].comID = port.comid;
    gRecvPacket[gRecvPortCnt].cycle = port.period*1000;
    gRecvPacket[gRecvPortCnt].timeout = 3000000;
    gRecvPacket[gRecvPortCnt].src = vos_dottedIP(port.srcipinfo.srcip.toStdString().c_str());
    gRecvPacket[gRecvPortCnt].dst = vos_dottedIP(port.destip.toStdString().c_str());
    gRecvPacket[gRecvPortCnt].receivedSize = port.size;
    gRecvPacket[gRecvPortCnt].comidIndex = gRecvPortCnt;
    memset(gRecvPacket[gRecvPortCnt].data,0,TRDP_MAX_PD_DATA_SIZE);
    port.srcipinfo.index = gRecvPortCnt;
    qDebug()<<"add recv src ip:"<<gRecvPacket[gRecvPortCnt].src<<" comid:"<<gRecvPacket[gRecvPortCnt].comID<<" dest ip:"<<
              gRecvPacket[gRecvPortCnt].dst<<" size:"<<gRecvPacket[gRecvPortCnt].receivedSize<<" cycle:"<<gRecvPacket[gRecvPortCnt].cycle<< "successfully";
    gRecvPortCnt++;
    RecvDataSize+=port.size;
}

void TRDPThread::add_pd_sourcePort(TRDPDataPortInfo &port)
{
    gSendPacket[gSendPortCnt].comID = port.comid;
    gSendPacket[gSendPortCnt].src = ownIP;
    gSendPacket[gSendPortCnt].dst = vos_dottedIP(port.destip.toStdString().c_str());
    gSendPacket[gSendPortCnt].sendSize = port.size;
    gSendPacket[gSendPortCnt].interval = port.period*1000;
    memset(gSendPacket[gSendPortCnt].data, 0, TRDP_MAX_PD_DATA_SIZE);
    SendDataSize+=port.size;
    qDebug()<<"add send port"<<port.srcipinfo.srcip<<""<<port.destip<<" "<<port.comid<<" "<<port.size<<" successfully";
    gSendPortCnt++;
}

void TRDPThread::initCom()
{
    for(int i=0;i<GlobalConfig::recvTRDPList.length();i++){
        if(GlobalConfig::recvTRDPList.at(i).type == RECV)
            add_pd_sinkPort(GlobalConfig::recvTRDPList[i]);
    }

    for(int i=0;i<GlobalConfig::sendTRDPList.length();i++){
        if(GlobalConfig::sendTRDPList.at(i).type == SEND)
            add_pd_sourcePort(GlobalConfig::sendTRDPList[i]);
    }
}
int TRDPThread::startCom()
{
    /*    Init the library  */
    if (tlc_init(&dbgOut,                              /* no logging    */
                 NULL,
                 &dynamicConfig) != TRDP_NO_ERR)    /* Use application supplied memory    */
    {
        printf("Initialization error\n");
        return 1;
    }

    /*    Open a session  */
    if (tlc_openSession(&appHandle,
                        ownIP, 0,               /* use default IP address           */
                        NULL,                   /* no Marshalling                   */
                        &pdConfiguration, NULL, /* system defaults for PD and MD    */
                        &processConfig) != TRDP_NO_ERR)
    {
        printf("Open session error\n");
        return 1;
    }

    /*    Subscribe to control PD        */
    for(int i=0;i<gRecvPortCnt;++i)
        pd_suscribe(appHandle,&gRecvPacket[i]);

    for(int i=0;i<gSendPortCnt;++i)
        pd_publish(appHandle,&gSendPacket[i]);

    err = tlc_updateSession(appHandle);
    if (err != TRDP_NO_ERR)
    {
        printf( "tlc_updateSession error (%s)\n", vos_getErrorString((VOS_ERR_T)err));
        tlc_terminate();
        return 1;
    }

    pthread_t threadID;
    pthread_mutex_init(&trdp_send_mutex,NULL);
    if(pthread_create(&threadID, NULL, comThread, (void*)this) != 0)
        qDebug() << "com thread creat failed!" << endl;
    return 0;
}

void* TRDPThread::comThread(void* arg)
{
    printf("------runinto trdp com thread------\n ");
    GlobalConfig::addOperationInfo(QString("TRDP通信在线程%1开始").arg(getpid()));
    TRDPThread* com = (TRDPThread*)arg;
    com->m_state = running;
    while (1)
     {
        int     rv      = 0;
        TRDP_FDS_T rfds;
        INT32 noDesc;
        TRDP_TIME_T tv = {0, 0};
        const TRDP_TIME_T   max_tv  = {1, 0};
        const TRDP_TIME_T   min_tv  = {0, TRDP_PROCESS_DEFAULT_CYCLE_TIME};
        FD_ZERO(&rfds);
        tlc_getInterval(com->appHandle, &tv, &rfds, &noDesc);
        if (vos_cmpTime(&tv, &max_tv) > 0)
        {
            tv = max_tv;
        }
        if (vos_cmpTime(&tv, &min_tv) < 0)
        {
            tv = min_tv;
        }
        rv = vos_select(noDesc + 1, &rfds, NULL, NULL, &tv);
        com-> err = tlc_process(com->appHandle, NULL, NULL);
         if (com->err != TRDP_NO_ERR)
         {
             printf("tlc_process returned: %s\n", vos_getErrorString((VOS_ERR_T)com->err));
         }

         for(int i=0;i<com->gRecvPortCnt;i++)
         {
             com->err = tlp_get(com->appHandle,
                                   com->gRecvPacket[i].subHandle,
                                   &com->gRecvPacket[i].pdi,
                                    com->gRecvPacket[i].data,
                                    &com->gRecvPacket[i].receivedSize);

             if (com->err != TRDP_NO_ERR )
             {
                 printf("Error on packet received (ComID %d), err = %d\n",com->gRecvPacket[i].comID,com->err);
             }
//             if(com->gRecvPacket[i].comID==10011)
//                 printf("device comid=%d, [%x] [%x] [%x] [%x] [%x] [%x] [%x] [%x]\n",
//                        com->gRecvPacket[i].comID,
//                        com->gRecvPacket[i].data[0],
//                        com->gRecvPacket[i].data[1],
//                         com->gRecvPacket[i].data[2],
//                         com->gRecvPacket[i].data[3],
//                         com->gRecvPacket[i].data[4],
//                         com->gRecvPacket[i].data[5],
//                         com->gRecvPacket[i].data[6],
//                         com->gRecvPacket[i].data[7]);
         }

         pthread_mutex_lock(&com->trdp_send_mutex);
         for(int i=0;i<com->gSendPortCnt;i++)
         {
//             if(i==0)
//                  printf("vcu hb = %d\n",com->gSendPacket[i].data[0]*256+com->gSendPacket[i].data[1]);
            com->err = tlp_put(com->appHandle,
                    com->gSendPacket[i].pubHandle,
                    com->gSendPacket[i].data,
                    com->gSendPacket[i].sendSize);
            if (com->err != TRDP_NO_ERR)
            {
                printf("Error on packet send (ComID %d), err = %d\n",
                       com->gSendPacket[i].comID,
                       com->err);
            }
         }
        pthread_mutex_unlock(&com->trdp_send_mutex);
    }
    com->stopCom();
    return NULL;
}

void TRDPThread::stopCom()
{
    for(int i=0;i<gRecvPortCnt;++i)
        tlp_unsubscribe(appHandle,gRecvPacket[i].subHandle);
    for(int i=0;i<gSendPortCnt;++i)
        tlp_unpublish(appHandle,gSendPacket[i].pubHandle);

    tlc_closeSession(appHandle);
    tlc_terminate();
    m_state = stop;
}

void TRDPThread::dbgOut(void *pRefCon,
                        TRDP_LOG_T  category,
                        const CHAR8 *pTime,
                        const CHAR8 *pFile,
                        UINT16      LineNumber,
                        const CHAR8 *pMsgStr)
{
    const char *catStr[] = {"**Error:", "Warning:", "   Info:", "  Debug:"};
           printf("%s %s %s:%d %s",
           strrchr(pTime, '-') + 1,
           catStr[category],
           strrchr(pFile, '/') + 1,
           LineNumber,
           pMsgStr);
    fflush(stdout);
}

void TRDPThread::pd_suscribe(TRDP_APP_SESSION_T &apph, PD_RECEIVE_PACKET_T *pd_recv_pack)
{
    TRDP_ERR_T err;
    err = tlp_subscribe(apph,                                 /*    our application identifier            */
                        &pd_recv_pack->subHandle,                    /*    our subscription identifier           */
                        NULL,                                   /*    user reference  */
                        NULL,                                   /*    callback function                    */
                        0u,                                         /*    serviceId                             */
                        pd_recv_pack->comID,                         /*    ComID                                 */
                        0,                                          /*    topocount: local consist only         */
                        0,
                        pd_recv_pack->src,                           /*    Source IP filter 1                    */
                        VOS_INADDR_ANY,
                        pd_recv_pack->dst,                            /*    Default destination    (or MC Group)  */
                        TRDP_FLAGS_DEFAULT,                    /*    Flags                                */
                        NULL,
                        pd_recv_pack->cycle*3,                         /*    Time out in us                        */
                        TRDP_TO_SET_TO_ZERO);                       /*  delete invalid data    on timeout       */

    if (err != TRDP_NO_ERR)
    {
        printf("trdp_subscribe error:%d  which comid = %d \n",err,pd_recv_pack->comID);
        tlc_terminate();
    }
}

void TRDPThread::pd_publish(TRDP_APP_SESSION_T &apph,PD_SEND_PACKET_T *pd_send_pack)
{
    TRDP_ERR_T  err;
    err = tlp_publish( apph,                          /*    our application identifier    */
                       &pd_send_pack->pubHandle,             /*    our pulication identifier     */
                       NULL, NULL,
                       0u,
                       pd_send_pack->comID,                  /*    ComID to send                 */
                       0u,                                  /*    local consist only            */
                       0u,
                       0u,                                  /*    default source IP             */
                       pd_send_pack->dst,                    /*    where to send to              */
                       pd_send_pack->interval,               /*    Cycle time in us              */
                       0u,                                  /*    not redundant                 */
                       TRDP_FLAGS_CALLBACK,                 /*    Use callback for errors       */
                       NULL,                                /*    default qos and ttl           */
                       (UINT8 *)pd_send_pack->data,          /*    initial data                  */
                       pd_send_pack->sendSize                /*    data size                     */
                       );                                   /*    no ladder                     */


   if (err != TRDP_NO_ERR)
   {
       printf( "prep pd publish error:%d\n",err);
       tlc_terminate();
   }
}

int TRDPThread::getComState()
{
    return m_state;
}

const unsigned char *TRDPThread::getRecvPointer(int comindex)
{
    return gRecvPacket[comindex].data;
}

unsigned char* TRDPThread::getSendPointer(int comindex)
{
    return gSendPacket[comindex].data;
}

int TRDPThread::getRecvSize()
{
    return RecvDataSize;
}

int TRDPThread::getSendSize()
{
    return SendDataSize;
}

unsigned int TRDPThread::getRecvStartByte(int index)
{
    Q_ASSERT(index < GlobalConfig::recvTRDPList.size());
    unsigned int startByte = 0;
    for(int i=0; i<index; ++i)
        startByte += GlobalConfig::recvTRDPList.at(i).size;
    return startByte;
}

unsigned int TRDPThread::getSendStartByte(int index)
{
    Q_ASSERT(index < GlobalConfig::sendTRDPList.size());
    unsigned int startByte = 0;
    for(int i=0; i<index; ++i)
        startByte += GlobalConfig::sendTRDPList.at(i).size;
    return startByte;
}
